#ifndef __DALELIB_ROBOTJOINT_H__
#define __DALELIB_ROBOTJOINT_H__

#include <dalelib/maths.h>
#include <ode/ode.h>

namespace dalelib
{

class Robot;

class RobotJoint
{
public:

	enum RobotJointType { BALLJOINT = 0, HINGEJOINT, SLIDERJOINT, UNIVERSALJOINT, };

protected:

	Robot* _parent;
	dJointID _id;
	RobotJointType _type;
	dBodyID _body[2];
	float _strength;
//	dJointFeedback _feedback;

public:

	RobotJoint();
	~RobotJoint();
	dJointID getID() const { return _id; }
	RobotJointType getType() const { return _type; }

//	v3 getAnchor() { dVector3 pos; dJointGetHingeAnchor(_id, pos); return v3(pos[0],pos[1],pos[2]); }
//	v3 getAxis() { dVector3 dir; dJointGetHingeAxis(_id, dir); return v3(dir[0],dir[1],dir[2]); }
//	v3 getTorque1() { return v3(_feedback.t1[0],_feedback.t1[1],_feedback.t1[2]); }
//	v3 getTorque2() { return v3(_feedback.t2[0],_feedback.t2[1],_feedback.t2[2]); }
//	dBodyID getBodyIndex(int _Index) { return _body[_Index]; }
//	float getStrength() { return _strength; }
//	void setLimits(const float _LoStop, const float _HiStop);
//	float getAngle() const { return float(dJointGetHingeAngle(_id)); }
//	void setVelocity(const float _Velocity);

	virtual void update() { }
	virtual void draw() { }

};



//**************************************************
struct RobotBallJoint : public RobotJoint
{
	RobotBallJoint(Robot* _Parent, const dBodyID _B0, const dBodyID _B1, const v3 _Anchor);
	void update();
	void draw();
};

//**************************************************
struct RobotHingeJoint : public RobotJoint
{
	RobotHingeJoint(Robot* _Parent, const dBodyID _B0, const dBodyID _B1, const v3 _Anchor, const v3 _Axis, const float _Strength);
	void setAngle(const float _Angle, const float _Force);
	v3 getAnchor() { dVector3 pos; dJointGetHingeAnchor(_id, pos); return v3(pos[0],pos[1],pos[2]); }
	v3 getAxis() { dVector3 dir; dJointGetHingeAxis(_id, dir); return v3(dir[0],dir[1],dir[2]); }
	void update();
	void draw();
};

//**************************************************
struct RobotSliderJoint : public RobotJoint
{
	RobotSliderJoint(Robot* _Parent, const dBodyID _B0, const dBodyID _B1, const v3 _Anchor, const v3 _Axis, const float _Strength);
	void update();
	void draw();
};

//**************************************************
struct RobotUniversalJoint : public RobotJoint
{
	RobotUniversalJoint(Robot* _Parent, const dBodyID _B0, const dBodyID _B1, const v3 _Anchor, const v3 _Axis, const float _Strength);
	void update();
	void draw();
};


} // namespace dalelib

#endif
